Example: Path planning problem solved by Uniform grid abstraction.

This example was borrowed from (Reissig et al., 2016; IX. Examples, A) whose dynamics comes from the model given in (Åström and Murray, 2007; Ch. 2.4). This is a reachability problem for a continuous system.

Let us consider the 3-dimensional state space control system of the form

\[\dot{x} = f(x, u)\]

with $f: \mathbb{R}^3 × U ↦ \mathbb{R}^3$ given by

\[f(x,(u_1,u_2)) = \begin{bmatrix} u_1 \cos(α+x_3)\cos(α)^{-1} \\ u_1 \sin(α+x_3)\cos(α)^{-1} \\ u_1 \tan(u_2) \end{bmatrix}\]

and with $U = [−1, 1] \times [−1, 1]$ and $α = \arctan(\tan(u_2)/2)$. Here, $(x_1, x_2)$ is the position and $x_3$ is the orientation of the vehicle in the 2-dimensional plane. The control inputs $u_1$ and $u_2$ are the rear wheel velocity and the steering angle. The control objective is to drive the vehicle which is situated in a maze made of obstacles from an initial position to a target position.

In order to study the concrete system and its symbolic abstraction in a unified framework, we will solve the problem for the sampled system with a sampling time $\tau$. For the construction of the relations in the abstraction, it is necessary to over-approximate attainable sets of a particular cell. In this example, we consider the used of a growth bound function (Reissig et al., 2016; VIII.2, VIII.5) which is one of the possible methods to over-approximate attainable sets of a particular cell based on the state reach by its center.

For this reachability problem, the abstraction controller is built by solving a fixed-point equation which consists in computing the pre-image of the target set.

First, let us import StaticArrays and Plots.

using StaticArrays, JuMP, Plots

using Dionysos
const DI = Dionysos
const ST = DI.System
const MP = DI.Mapping
const SY = DI.Symbolic
const OP = DI.Optim
const AB = OP.Abstraction

Dionysos.Optim.Abstraction

Definition of the problem

We define the problem using JuMP as follows. We first create a JuMP model:

model = Model(Dionysos.Optimizer);

Define the state variables: x1(t), x2(t), x3(t)

x_low, x_upp = [0.0, 0.0, -pi - 0.4], [4.0, 10.0, pi + 0.4]
x_start = [0.4, 0.4, 0.0]
@variable(model, x_low[i] <= x[i = 1:3] <= x_upp[i], start = x_start[i]);

Define the control variables: u1(t), u2(t)

@variable(model, -1 <= u[1:2] <= 1);

Set α(t) = arctan(tan(u2(t)) / 2)

@expression(model, α, atan(tan(u[2]) / 2))

@constraint(model, ∂(x[1]) == u[1] * cos(α + x[3]) * sec(α))
@constraint(model, ∂(x[2]) == u[1] * sin(α + x[3]) * sec(α))
@constraint(model, ∂(x[3]) == u[1] * tan(u[2]))

x_target = [3.3, 0.5, 0]

@constraint(model, final(x[1]) in MOI.Interval(3.0, 3.6))
@constraint(model, final(x[2]) in MOI.Interval(0.3, 0.8))
@constraint(model, final(x[3]) in MOI.Interval(-100.0, 100.0))

\[ \textsf{final}\left({x_{3}}\right) \in [-100, 100] \]

Obstacle boundaries (provided)

x1_lb = [1.0, 2.2, 2.2]
x1_ub = [1.2, 2.4, 2.4]
x2_lb = [0.0, 0.0, 6.0]
x2_ub = [9.0, 5.0, 10.0];

Function to add rectangular obstacle avoidance constraints

for i in eachindex(x1_ub)
    @constraint(
        model,
        x[1:2] ∉ MOI.HyperRectangle([x1_lb[i], x2_lb[i]], [x1_ub[i], x2_ub[i]])
    )
end

Definition of the abstraction

Definition of the grid of the state-space on which the abstraction is based (origin x0 and state-space discretization h):

We define the growth bound function of $f$:

function jacobian_bound_function(u)
    β = abs(u[1] / cos(atan(tan(u[2]) / 2)))
    return StaticArrays.SMatrix{3, 3}(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, β, β, 0.0)
end
set_attribute(model, "jacobian_bound", jacobian_bound_function)
set_attribute(model, "time_step", 0.3)
set_attribute(model, "approx_mode", AB.UniformGridAbstraction.GROWTH)
set_attribute(model, "efficient", true)

x0 = SVector(0.0, 0.0, 0.0);
hx = SVector(0.2, 0.2, 0.2);
set_attribute(model, "state_grid", MP.GridFree(x0, hx))

Definition of the grid of the input-space on which the abstraction is based (origin u0 and input-space discretization h):

u0 = SVector(0.0, 0.0);
hu = SVector(0.3, 0.3);
set_attribute(model, "input_grid", MP.GridFree(u0, hu))

optimize!(model);

>>Setting up the model
>>Model setup complete
[ Info: Number of states: 26285
[ Info: Number of inputs: 49
[ Info: Number of forward images: 1287965
┌ Warning: Noise is not yet accounted for in system abstraction.
└ @ Dionysos.Optim.Abstraction.UniformGridAbstraction ~/.julia/packages/Dionysos/JIiFW/src/optim/abstraction/UniformGridAbstraction/empty_problem.jl:465
compute_abstract_system_from_concrete_system!: started
compute_abstract_system_from_concrete_system! terminated with success: 9705631 transitions created
┌ Warning: The `state_cost` is not yet fully implemented
└ @ Dionysos.Optim.Abstraction.UniformGridAbstraction ~/.julia/packages/Dionysos/JIiFW/src/optim/abstraction/UniformGridAbstraction/optimal_control_problem.jl:233
compute_controller_reachability! started

 Reachability: terminated with false

Get the results

abstract_system = get_attribute(model, "abstract_system");
abstract_problem = get_attribute(model, "abstract_problem");
abstract_controller = get_attribute(model, "abstract_controller");
concrete_controller = get_attribute(model, "concrete_controller")
concrete_problem = get_attribute(model, "concrete_problem");
abstract_value_function = get_attribute(model, "abstract_value_function")
concrete_system = concrete_problem.system
abstraction_time =
    MOI.get(model, MOI.RawOptimizerAttribute("abstraction_construction_time_sec"))
println("Time to construct the abstraction: $(abstraction_time)")
abstract_problem_time =
    MOI.get(model, MOI.RawOptimizerAttribute("abstract_problem_time_sec"))
println("Time to solve the abstract problem: $(abstract_problem_time)")
total_time = MOI.get(model, MOI.RawOptimizerAttribute("solve_time_sec"))
println("Total time: $(total_time)")

Time to construct the abstraction: 2.6834168434143066
Time to solve the abstract problem: 0.8489930629730225
Total time: 3.5324699878692627

Trajectory display

We choose a stopping criterion reached and the maximal number of steps nsteps for the sampled system, i.e. the total elapsed time: nstep*tstep as well as the true initial state x0 which is contained in the initial state-space _I_ defined previously.

nstep = 100
reached(x) = x ∈ concrete_problem.target_set

x0 = SVector(0.4, 0.4, 0.0)
x_traj, u_traj = ST.get_closed_loop_trajectory(
    get_attribute(model, "discrete_time_system"),
    concrete_controller,
    x0,
    nstep;
    stopping = reached,
)

using Plots

Here we display the coordinate projection on the two first components of the state space along the trajectory.

fig = plot(; aspect_ratio = :equal);

We display the concrete domain

plot!(concrete_system.X; color = :grey, opacity = 1.0, label = "");

We display the abstract domain with worst-case cost

plot!(abstract_system; value_function = abstract_value_function);

We display the concrete specifications

plot!(concrete_problem.initial_set; color = :green, opacity = 0.2, label = "Initial set");
plot!(
    concrete_problem.target_set;
    dims = [1, 2],
    color = :red,
    opacity = 0.5,
    label = "Target set",
);

We display the abstract specifications

Xmapping = SY.get_state_mapping(abstract_system)
plot!(
    (SY.get_state_set_from_states(abstract_system, abstract_problem.initial_set), Xmapping);
    color = :green,
);
plot!(
    (SY.get_state_set_from_states(abstract_system, abstract_problem.target_set), Xmapping);
    color = :red,
    efficient = false,
);

We display the concrete trajectory

plot!(x_traj; ms = 2.0, arrows = false)

This page was generated using Literate.jl.